//----- Include Files ---------------------------------------------------------
#include <avr/io.h>	// include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h>

#include "robbus.h"

#define INDEX_FLAGS 0
#define INDEX_PWM 1

#define LEFT_INDICATOR_MASK 1
#define RIGHT_INDICATOR_MASK 2

// indicator periods (cca 30 = 1ms)
#define INDICATOR_VALUE 10000
#define INDICATOR_TOP_VALUE 30000

// flags (3bit on time, 3bit off time, right indicator, left indicator) 
// main pwm
static uint8_t outData[ROBBUS_OUTGOING_SIZE];

static volatile uint8_t g_ticks = 0;

static volatile uint8_t g_onTime = 1;
static volatile uint8_t g_offTime = 0;
static volatile uint8_t g_pwm = 2; // TODO change to zero in final version
static uint8_t g_isBrake = 0;

ISR(TIMER0_OVF_vect) {
	g_ticks += 1;
}

static void setMainLight(uint8_t value) {
	if (value > 0) {
		TCCR2 |= (1<<COM21); 
		OCR2 = value;
	} else {
		TCCR2 &= ~(1<<COM21); 
		OCR2 = 0;
	}
}

static void setLeftIndicator(uint8_t value) {
	if (value) {
		TCCR1A |= (1<<COM1A1); 
		OCR1A = INDICATOR_VALUE;
	} else {
		TCCR1A &= ~(1<<COM1A1); 
		OCR1A = 0;
	}
}

static void setRightIndicator(uint8_t value) {
	if (value) {
		TCCR1A |= (1<<COM1B1); 
		OCR1B = INDICATOR_VALUE;
	} else {
		TCCR1A &= ~(1<<COM1B1);
		OCR1B = 0; 
	}
}

static uint8_t* messageHandler(uint8_t *inData) {
	uint8_t flags = inData[INDEX_FLAGS];
	setLeftIndicator(flags & LEFT_INDICATOR_MASK);
	setRightIndicator(flags & RIGHT_INDICATOR_MASK);
	g_onTime = (flags >> 2) & 7; // main light blink time
	g_offTime = 3 * ((flags >> 5) & 7);

	g_pwm = inData[INDEX_PWM];
	setMainLight(g_pwm);

	outData[0] = g_isBrake;
	return outData;
}

static void init(void)
{
	// initialize library units
	Robbus_Init(messageHandler);

	DDRB |=  (1<<PB1) | (1<<PB2) | (1<<PB3); // OC1A, OC1B, OC2 as outputs

	// initialize timer 0 for flashing timing
	//TCCR0 = (1<<CS02) | (1<<CS00); // 1:1024
	TCCR0 = (1<<CS02); // 1:256
	TIMSK |= (1<<TOIE0); // enable owerflow interrupt	

	// initialize timer 1 for indicator lights
	TCNT1 = 0; // clean timer
	ICR1  = INDICATOR_TOP_VALUE; 
	TCCR1A = (1<<WGM11); // fast pwm
	TCCR1B = (1<<WGM13)  | (1<<WGM12)  | (1<<CS11) | (1<<CS10) ; // prescaler to 1:64

	// initialize timer 2 for main light block
	TCNT2 = 0;
	TCCR2 = (1<<WGM21) | (1<< WGM20) | (1<<CS22); // fast pwm 1:64
	setMainLight(g_pwm);

	// enable interrupts
	
	sei();
}

//----- Begin Code ------------------------------------------------------------


int main(void)
{
	init();

	while(1) {
		if (PINC & (1<<PC0)) { // no break
			if (g_isBrake) { // if was brake, reset status
				g_isBrake = 0;
				setMainLight(g_pwm);
				g_ticks = 0;
			}
			if (OCR2 > 0 && g_offTime > 0) { // currently on and blink mode
				if (g_ticks > g_onTime) { // on time elapsed, switch to off
					g_ticks = 0;
					setMainLight(0);
				}
			} else if (OCR2 == 0) { // currently off
				if (g_ticks > g_offTime) { // off time elapsed, switch to on
					g_ticks = 0;
					setMainLight(g_pwm);
				}
			}
		}
		else { // brake;
			if (g_isBrake == 0) { // not brake before
				g_isBrake += 1;
				TCNT0 = 0;
				g_ticks = 0;
			} else if (g_isBrake < 10) {
				if (g_ticks >= 2*g_isBrake) {
					g_isBrake += 1;
					setMainLight((g_isBrake % 2) == 0 ? 255 : 0);
				}
			} else {
				setMainLight(255);
			}
		}
		// nothing to do in main loop
		asm volatile("wdr");
	}

	return 0;
}
